#include "Face.h"
#include <cmath>
#include <iostream>

Face::Face(int id) : faceId(id) {	
}

Face::~Face() {
}

void Face::addLandmarks(map<LandmarkType, Point>& landmarks, Point faceStartPos, Point faceEndPos) {
	map<LandmarkType, Point>::const_iterator it;
	for (it = landmarks.begin(); it != landmarks.end(); it++) {
		Point point = it->second;
		
		// Calculate radius
		float radius = (float)sqrt((point.getX() - faceStartPos.getX()) * (point.getX() - faceStartPos.getX()) + 
							(point.getY() - faceStartPos.getY()) * (point.getY() - faceStartPos.getY()));
		
		// Calculate angle
		float angle = (float)atan2(point.getY() - faceStartPos.getY(), point.getX() - faceStartPos.getX());
		
		LandmarkType landmarkType = it->first;
		
		// Create new position object and place that object in list of landmarks
		Landmark landmark(radius, angle);
		listOfLandmarks.insert(pair<LandmarkType, Landmark>(landmarkType, landmark));
	}
}

void Face::calculatePairs() {
	/*
	FOREHEAD = 0,
	CHIN = 1,
	EYES_MIDDLE = 2,
	LEFT_EYE_LEFT_POINT = 3,
	LEFT_EYE_RIGHT_POINT = 4,
	RIGHT_EYE_LEFT_POINT = 5,
	RIGHT_EYE_RIGHT_POINT = 6,
	NOSE_LEFT_POINT = 7,
	NOSE_MIDDLE_POINT = 8,
	NOSE_RIGHT_POINT = 9,
	MOUTH_LEFT_POINT = 10,
	MOUTH_RIGHT_POINT = 11,
	FACE_BOTTOM_LEFT = 12,
	FACE_BOTTOM_RIGHT = 13
	*/
	
	// FOREHEAD - EYES MIDDLE
	// Calculate radius ratio
	double radius = listOfLandmarks[FOREHEAD].getRadius() / listOfLandmarks[EYES_MIDDLE].getRadius();
	// Calculate angle offset
	double angle = abs(listOfLandmarks[FOREHEAD].getAngle() - listOfLandmarks[EYES_MIDDLE].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// FOREHEAD - LEFT EYE LEFT POINT
	// Calculate radius ratio
	radius = listOfLandmarks[FOREHEAD].getRadius() / listOfLandmarks[LEFT_EYE_LEFT_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[FOREHEAD].getAngle() - listOfLandmarks[LEFT_EYE_LEFT_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// LEFT EYE LEFT POINT - LEFT EYE RIGHT POINT
	// Calculate radius ratio
	radius = listOfLandmarks[FOREHEAD].getRadius() / listOfLandmarks[LEFT_EYE_RIGHT_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[FOREHEAD].getAngle() - listOfLandmarks[LEFT_EYE_RIGHT_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// EYES MIDDLE - NOSE MIDDLE POINT
	// Calculate radius ratio
	radius = listOfLandmarks[EYES_MIDDLE].getRadius() / listOfLandmarks[NOSE_MIDDLE_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[EYES_MIDDLE].getAngle() - listOfLandmarks[NOSE_MIDDLE_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// EYES MIDDLE - NOSE LEFT POINT
	// Calculate radius ratio
	radius = listOfLandmarks[EYES_MIDDLE].getRadius() / listOfLandmarks[NOSE_LEFT_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[EYES_MIDDLE].getAngle() - listOfLandmarks[NOSE_LEFT_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// NOSE LEFT POINT - NOSE RIGHT POINT
	// Calculate radius ratio
	radius = listOfLandmarks[NOSE_LEFT_POINT].getRadius() / listOfLandmarks[NOSE_RIGHT_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[NOSE_LEFT_POINT].getAngle() - listOfLandmarks[NOSE_RIGHT_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// NOSE MIDDLE POINT - CHIN
	// Calculate radius ratio
	radius = listOfLandmarks[NOSE_MIDDLE_POINT].getRadius() / listOfLandmarks[CHIN].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[NOSE_MIDDLE_POINT].getAngle() - listOfLandmarks[CHIN].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// NOSE MIDDLE POINT - MOUTH LEFT POINT
	// Calculate radius ratio
	radius = listOfLandmarks[NOSE_MIDDLE_POINT].getRadius() / listOfLandmarks[MOUTH_LEFT_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[NOSE_MIDDLE_POINT].getAngle() - listOfLandmarks[MOUTH_LEFT_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// MOUTH LEFT POINT - MOUTH RIGHT POINT
	// Calculate radius ratio
	radius = listOfLandmarks[MOUTH_LEFT_POINT].getRadius() / listOfLandmarks[MOUTH_RIGHT_POINT].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[MOUTH_LEFT_POINT].getAngle() - listOfLandmarks[MOUTH_RIGHT_POINT].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	
	// MOUTH LEFT POINT - CHIN
	// Calculate radius ratio
	radius = listOfLandmarks[MOUTH_LEFT_POINT].getRadius() / listOfLandmarks[CHIN].getRadius();
	// Calculate angle offset
	angle = abs(listOfLandmarks[MOUTH_LEFT_POINT].getAngle() - listOfLandmarks[CHIN].getAngle());
	pairs.push_back(radius); pairs.push_back(angle);
	// Calculated position
				
				// Add to list of pairs
				// pairs.push_back(complex<float>(radius * cos(angle), radius * sin(angle)));
				
				// 0 - 2
				// 0 - 3
				// 3 - 4
				// 2 - 8
				// 2 - 7
				// 7 - 9
				// 8 - 1
				// 8 - 10
				// 10 - 11
				// 10 - 1
}

vector<double>& Face::getPairs() {
	return pairs;
}

int Face::getFaceId() {
	return faceId;
}
